// function prototype
void wait1ms(void);
#define F_CPU 8000000UL	      // Sets up the default speed for delay.h

// Include definition file
#include <ioavr.h>
#include <inavr.h>
#include <STDIO.H>
#include <STDLIB.H>
#include <string.H>
#include "USART2.h"
//#include "USI_TWI_Master.h"


/// Typedefs //////////
typedef unsigned char u8;
typedef unsigned int u16;
typedef unsigned long u32;

#define _BV(x) (1<<x)

#define ACCEL_ADDR  0x1d
#define MESSAGEBUF_SIZE       4

//extern char _low_level_get(void){
// return USART_Receive() ;
//}

unsigned char TWI_Act_On_Failure_In_Last_Transmission ( unsigned char TWIerrorMsg )
{
                    // A failure has occurred, use TWIerrorMsg to determine the nature of the failure
                    // and take appropriate actions.
                    // Se header file for a list of possible failures messages.

  __no_operation();
                     
  return TWIerrorMsg; 
}
/* reverse:  reverse string s in place */
void reverse(char s[])
{
    int c, i, j;

    for (i = 0, j = strlen(s)-1; i<j; i++, j--) {
        c = s[i];
        s[i] = s[j];
        s[j] = c;
    }
}

/* itoa:  convert n to characters in s */
void itoa(int n, char s[])
{
    int i, sign;

    if ((sign = n) < 0)  /* record sign */
        n = -n;          /* make n positive */
    i = 0;
    do {       /* generate digits in reverse order */
        s[i++] = n % 10 + '0';   /* get next digit */
    } while ((n /= 10) > 0);     /* delete it */
    if (sign < 0)
        s[i++] = '-';
    s[i] = '\0';
    reverse(s);
} 

void print(char *str)
{
	int i=0;
	while (str[i]!=0){
		USART_Transmit(str[i]);
		i++;
	}
}

void sleep(float msPeriod){
	for (int i=0;i<msPeriod;i++)
		wait1ms();
}

enum { UP, DOWN };

void SPI_MasterInit(void)
{
  volatile char IOReg;
  // set PB4(/SS), PB5(MOSI), PB7(SCK) as output
  DDRB = (1<<PB4)|(1<<PB5)|(1<<PB7);
  /* Enable SPI, Master, set clock rate fck/16 */
  SPCR = (1<<SPE)|(1<<MSTR)|(1<<CPOL)|(1<<CPHA)|(1<<SPR0);
  IOReg   = SPSR;                 	// clear SPIF bit in SPSR
  IOReg   = SPDR;
}

char SPI_MasterTransmit(char cData)
{
  /* Start transmission */
  SPDR = cData;
  /* Wait for transmission complete */
  while(!(SPSR & (1<<SPIF)));
  return SPDR;
}

// reads a register
char read_register(char register_name)
{
   char in_byte;
   register_name |= 1<<7;
   PORTC &= ~_BV(PORTC0);
   SPI_MasterTransmit(register_name);
   in_byte = SPI_MasterTransmit(0);
   PORTC |= _BV(PORTC0); 
   return in_byte;
}

// write to a register
void write_register(char register_name, char data)
{
   register_name &= 1<<7;
   PORTC &= ~_BV(PORTC0);
   SPI_MasterTransmit(register_name);
   SPI_MasterTransmit(data);
   PORTC |= _BV(PORTC0);
}


int main( void )
{
  USART_Init(51);
  SPI_MasterInit();
  __enable_interrupt();
  DDRC = 255;
  DDRD = 255;	
  PORTD = 0; 
  print("\r\nInitializing...\r\n");
  sleep(200);
  PORTD = 255;
  print("Running.\r\n");
  
  char result;
  char str[8];
  result=read_register(0xf);
  itoa(result,str);
  print(str);
  
  while(1){
  print(">");
      u8 rcv=USART_Receive();
      if (rcv>='0' && rcv<='9'){
              USART_Transmit(rcv);
              PORTD = ~_BV(rcv-'0');
              //OCR = (rcv-'0')*100;
      }
       print("\r\n");
  };
  /*
  unsigned char messageBuf[MESSAGEBUF_SIZE];
  unsigned char TWI_targetSlaveAddress;
  USI_TWI_Master_Initialise();
  TWI_targetSlaveAddress   = ACCEL_ADDR;

  messageBuf[0] = (TWI_targetSlaveAddress<<TWI_ADR_BITS) | (FALSE<<TWI_READ_BIT);
  messageBuf[1] = 0x20;
  messageBuf[2] = 0xc7;
  USI_TWI_Start_Transceiver_With_Data( messageBuf, 3 );
  sleep(100);
  u8 x1,x2;
  char buf[10];
  while (1)
  {
    messageBuf[0] = (TWI_targetSlaveAddress<<TWI_ADR_BITS) | (FALSE<<TWI_READ_BIT);
    messageBuf[1] = 0x28;
    USI_TWI_Start_Transceiver_With_Data( messageBuf, 2 );
    messageBuf[0] = (TWI_targetSlaveAddress<<TWI_ADR_BITS) | (TRUE<<TWI_READ_BIT);
    USI_TWI_Start_Transceiver_With_Data( messageBuf, 2 );  
    x1=messageBuf[1];
    
    messageBuf[0] = (TWI_targetSlaveAddress<<TWI_ADR_BITS) | (FALSE<<TWI_READ_BIT);
    messageBuf[1] = 0x29;
    USI_TWI_Start_Transceiver_With_Data( messageBuf, 2 );
    messageBuf[0] = (TWI_targetSlaveAddress<<TWI_ADR_BITS) | (TRUE<<TWI_READ_BIT);
    USI_TWI_Start_Transceiver_With_Data( messageBuf, 2 );
    x2=messageBuf[1];
    itoa(x1|x2<<8,buf);
    print("x:");
    print(buf);
    
    messageBuf[0] = (TWI_targetSlaveAddress<<TWI_ADR_BITS) | (FALSE<<TWI_READ_BIT);
    messageBuf[1] = 0x2a;
    USI_TWI_Start_Transceiver_With_Data( messageBuf, 2 );
    messageBuf[0] = (TWI_targetSlaveAddress<<TWI_ADR_BITS) | (TRUE<<TWI_READ_BIT);
    USI_TWI_Start_Transceiver_With_Data( messageBuf, 2 );  
    x1=messageBuf[1];
    messageBuf[0] = (TWI_targetSlaveAddress<<TWI_ADR_BITS) | (FALSE<<TWI_READ_BIT);
    messageBuf[1] = 0x2b;
    USI_TWI_Start_Transceiver_With_Data( messageBuf, 2 );
    messageBuf[0] = (TWI_targetSlaveAddress<<TWI_ADR_BITS) | (TRUE<<TWI_READ_BIT);
    USI_TWI_Start_Transceiver_With_Data( messageBuf, 2 );
    x2=messageBuf[1];
    itoa(x1|x2<<8,buf);
    print(" y:");
    print(buf);
    
    messageBuf[0] = (TWI_targetSlaveAddress<<TWI_ADR_BITS) | (FALSE<<TWI_READ_BIT);
    messageBuf[1] = 0x2c;
    USI_TWI_Start_Transceiver_With_Data( messageBuf, 2 );
    messageBuf[0] = (TWI_targetSlaveAddress<<TWI_ADR_BITS) | (TRUE<<TWI_READ_BIT);
    USI_TWI_Start_Transceiver_With_Data( messageBuf, 2 );  
    x1=messageBuf[1];
    messageBuf[0] = (TWI_targetSlaveAddress<<TWI_ADR_BITS) | (FALSE<<TWI_READ_BIT);
    messageBuf[1] = 0x2d;
    USI_TWI_Start_Transceiver_With_Data( messageBuf, 2 );
    messageBuf[0] = (TWI_targetSlaveAddress<<TWI_ADR_BITS) | (TRUE<<TWI_READ_BIT);
    USI_TWI_Start_Transceiver_With_Data( messageBuf, 2 );
    x2=messageBuf[1];
    itoa(x1|x2<<8,buf);
    print(" z:");
    print(buf);
    USART_Transmit('\n');
    //itoa(x2,buf);
    //print(buf);
    //USART_Transmit('\n');
    
    sleep(100);
    */
    /* printf(">");
      u8 rcv=USART_Receive();
      if (rcv>='0' && rcv<='9'){
              //TxByte(rcv-'0');
              PORTD = ~_BV(rcv-'0');
              //OCR = (rcv-'0')*100;
      }
      printf("%c\n",rcv);    */
  //} 
}




// *** Primitive wait() function ***
void wait1ms(void){
  __delay_cycles(F_CPU/1000);
}
